#ifndef KALMAN_H
#define KALMAN_H

#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui.hpp>
#include <cassert>
#include <chrono>
#include <queue>
#include <thread>
#include "manifoldCommunication.h"

using namespace cv;
using namespace Eigen;
namespace robowalker{
    class KalmanPredictor
    {
    public:
        KalmanPredictor(const double Dt = 5); // Dt: ms
        void initKF();
        void reset();
        Mat update(Mat measurement, Mat U = Mat_<double>(2, 1));
        pair<double, double> velocityPredict() const;
        void addMeasurementToQueue(const double angleX_measurement, const double angleY_measurement);
        void clearMeasurementQueue(); 
    private:
        KalmanFilter KF;
        // X: state: (horizontal angle(yaw), vertical angle(pitch), horizontal velocity, vertical velocity)
        deque<double> angleX_measurement_queue;
        deque<double> angleY_measurement_queue;
        Mat B;//4*2
        float dt;
    };

    //thread function
    //TODO: use struct to pass infomation 
    void thread_kalmanPredictor(const Mat* angleMeasurement, Mat* predict_angle, const bool* ptr_reset, const double* dist, car_info* car_data, int* detectState);
}

#endif // KALMAN_H